/*
	Decoder.cpp - Class for Handling LS7266R1 Decoder on Atmel Microcontroller

	Makes assumption that only the X axes will be used

	Created by the LSI Capstone team at UMBC on March 11, 2014


 */

#include "Arduino.h"
#include "Decoder.h"

#define DIRECTION_REG DDRK
#define INPUT_REG PINK
#define OUTPUT_REG PORTK


#define XDATA(arg) (arg | 0)     	// 
#define XCMD(arg) (arg | 1) 			// 
#define YDATA(arg) (arg | 2) 		// 
#define YCMD(arg) (arg | 3) 			// 

// Reset and Load Signal Decoder Registers
#define RLD(arg) (arg | 0x80)  		// Reset and Load Decoder
#define XRLD(arg) (arg | 0)
#define YRLD(arg) XRLD(arg)
#define Rst_BP 0x01 				// Reset Byte Pointer
#define Rst_CNTR 0x02 				// Reset Counter
#define Rst_FLAGS 0x04 				// Reset Flags
#define Rst_E 0x06 					// Reset Error?
#define Trf_PR_CNTR 0x08 			// Transfer Preset Register to Counter
#define Trf_CNTR_OL 0x10 			// Transfer Counter to Output Latch
#define Trf_PS0_PSC 0x18 			// Transfer ?

// Counter Mode Registers
#define CMR(arg) (arg | 0xA0)		// Select Both X and Y CMRs
#define XCMR(arg) (arg | 0x20)		// Select X Counter Mode Register by (X)/Y input
#define YCMR(arg) XCMR(arg) 		// Select Y Counter Mode Register
#define BINCnt 0x00 				// Binary Count Mode
#define BCDCnt 0x01  				// BCD Count Mode
#define NrmCnt 0x00 				// Normal Count Mode
#define RngLmt 0x02 				// Range-limit Count Mode (upper and lower limit set) 
										// (freezes counter at CNTR=PR upper limit and CNTR=0 lower limit)
#define NRcyc 0x04  				// Non-Recycle Mode (see p.3)
#define ModN 0x06 					// Module-N Count Mode (see p.3)
#define NQDX 0x00 					// Non-Quadrature
#define QDX1 0x08 					// Quadrature x1
#define QDX2 0x10 					// Quadrature x2
#define QDX4 0x18 					// Quadrature x4

// Input-Output Control Registers
#define IOR(arg) (arg | 0xC0) 		// Select Both X and Y IORs
#define XIOR(arg) (arg | 0x40) 		// Select IOR addressed by (X)/Y input
#define YIOR(arg) XIOR(arg) 		// 
#define DisAB 0x00 					// Disable Inputs A and B
#define EnAB 0x01 					// Enable Inputs A and B
#define LCNTR 0x00 					// Load Counter Input
#define LOL 0x02 					// Load OL Input
#define RCNTR 0x00 					// Reset Counter Input
#define ABGate 0x04 				// A and B Enable gate
#define CYBW 0x00 					// Flag1 is Carry output, Flag2 is Borrow output
#define CPBW 0x08 					// Flag1 is Compare output, Flag2 is Borrow output
#define CB_UPDN 0x10 				// Flag1 is Carry/Borrow output, Flag2 is Up/Dn
#define IDX_ERR 0x18 				// Flag1 is IDX, Flag2 is E

// Index Control Registers
#define IDR(arg) (arg | 0xE0) 		// Select Both X and Y Index Control Registers
#define XIDR(arg) (arg | 0x60)		// Select X IDR
#define YIDR(arg) XIDR(arg)			// Select Y IDR
#define DisIDX 0x00					// Disable Index
#define EnIDX 0x01  				// Enable Index
#define NIDX 0x00 					// Negative Index Polarity
#define PIDX 0x02 					// Positive Index Polarity
#define LIDX 0x00 					// (LCNTR)/(LOL) Pin is Indexed
#define RIDX 0x04 					// (RCNTR)/(ABG) Pin is Indexed

#define DELAY_MICRO 2



/*
 * Decoder constructor
 *
 * Setup control pins on Arduino
 */
Decoder::Decoder()
//Decoder::Decoder(int oPin_CD, int oPin_WR, int oPin_RD, int oPin_FLG1, 
//	int oPin_FLG2, int oPin_FCLK)
{
	this->Addr = 0x0;

	this->oPin_FCLK = 11;
	this->oPin_RD = 10;
	this->oPin_WR = 9;
	this->oPin_CD = 8;
	this->oPin_CS = 7;

	this->iPin_FLG2 = 13;
	this->iPin_FLG1 = 12;
	
	//DEBUG
	this->countDEBUG = 0;

	// Set Pins on uC
	pinMode(this->oPin_CD, OUTPUT);
	pinMode(this->oPin_WR, OUTPUT);
	pinMode(this->oPin_RD, OUTPUT);
	pinMode(this->oPin_CS, OUTPUT);

	//pinMode(this->iPin_FLG1, INPUT);
	//pinMode(this->iPin_FLG2, INPUT);


	// Configure Decoder Internals
	//initializeDecoder();
}


void Decoder::startClock()
{
	// Configure Clock (Assume Timer 1)
	pinMode(this->oPin_FCLK, OUTPUT);  // Set OC1A pin as output
	TCCR1A = 0; // Clear register
	TCCR1A = (1 << COM1A0)  ;  // COM1A - Toggle A
	TCCR1B = 0; // Clear register
	TCCR1B = (1 << CS10) | (1 << WGM12) ; // CS10 - no prescaling,  WGM12 - CTC
	OCR1A = 0x3;  // Set compare reg. to 0 for max frequency  (F_clk / 2)  (0x1: 4 MHz, 0x3: 2 MHz)
	delayMicroseconds(2);
}


/*
 * InitializeDecoder
 *
 * Initialize 7266 as follows (X + Y CNTR)
 * Modulo N count mode for N = 0x123456
 * Binary Counting
 * Index on LCNTR/LOL Input
 * CY and BW outputs
 * RCNTR/ABG controls Counters
 * A and B Enabled
 */
void Decoder::initializeDecoder()
{	
	// Setup "Input-Output Control" Registers
	outp(XCMD(this->Addr), IOR(DisAB | LOL | ABGate | CPBW)); // Disable Counters and Set CY BW Mode

	// Setup "Reset and Load Signal Decoder" Register
	outp(XCMD(this->Addr), RLD(Rst_BP | Rst_FLAGS)); 	// Reset Byte Pointer (BP) and Flags
	outp(XDATA(this->Addr), 0x01); 						// Load 6 to PR0 to setup Transfer of PS0
	outp(XCMD(this->Addr), RLD(Rst_E | Trf_PS0_PSC)); 	// Reset E Flag and Transfer PR0 to PSC

	// Setup "Index Control" Register
	//outp(XCMD(this->Addr), IDR(EnIDX | NIDX | LIDX)); // Enable Negative Index on LCNTR/LOL Input

	// Setup "Counter Mode" Register
	outp(XCMD(this->Addr), CMR(BINCnt | ModN | QDX1)); // Set Binary Modulo N Quadrature x1

	// Setup PR Register for Modulo N Counter to 0x123456
	setCounter(0xFFFF0);
	
	// Enable Counters
	outp(XCMD(this->Addr), IOR(EnAB));
}

//--------------//--------------//--------------//--------------//--------------

void Decoder::disableAB()
{
	outp(XCMD(this->Addr), IOR(DisAB));
}

void Decoder::enableAB()
{
	outp(XCMD(this->Addr), IOR(EnAB));
}


/*
 * Read
 * 
 * Input: Addr has Address of 7266 counter
 * Ouput: Data returns 24 bit OL register value
 */
unsigned long Decoder::readCounter()
{
	unsigned long Data = 0;
	outp(XCMD(this->Addr), RLD(Rst_BP | Trf_CNTR_OL)); 	// Reset Byte Pointer and 
															// Transfer Counter to Output Latch

	// Reset Byte Pointer to Synchronize Byte reading and Transferring of data from counters to OL.
	Data |= (unsigned long) inp(XDATA(this->Addr));		// Read byte 0 from OL
	Data = lrotr(Data, 8); 							// Rotate for next byte
	Data |= (unsigned long) inp(XDATA(this->Addr));		// Read byte 1 from OL
	Data = lrotr(Data, 8); 							// Rotate for next byte
	Data |= (unsigned long) inp(XDATA(this->Addr));		// Read byte 2 from OL
	Data = lrotr(Data, 16);  						// Rotate for last byte
	return (Data);
}


void Decoder::setCounter(unsigned long Data)
{
	outp(XCMD(this->Addr), RLD(Rst_BP | Rst_CNTR));
	writeData(Data);
	outp(XCMD(this->Addr), RLD(Trf_PR_CNTR));
}

unsigned long Decoder::connectionTest(unsigned long Data)
{
	unsigned long Data2;
	outp(XCMD(this->Addr), IOR(DisAB));
	setCounter(Data);
	Data2 = readCounter();
	outp(XCMD(this->Addr), IOR(EnAB));
	return Data2;
}

//--------------//--------------//--------------//--------------//--------------


/*
 * WriteData
 *
 * Input: Addr has Address of 7266 counter
 * Data: has 24 bit data to be written to PR register
 */
void Decoder::writeData(unsigned long Data)
{
	outp(XCMD(this->Addr), RLD(Rst_BP)); 				// Reset Byte Pointer to Synchronize Byte Writing
	outp(XDATA(this->Addr), (unsigned char) Data);		// Write Low Bit
	Data >>= 8;
	outp(XDATA(this->Addr), (unsigned char) Data);		// Write Middle Bit
	Data >>= 8;
	outp(XDATA(this->Addr), (unsigned char) Data);		// Write High Bit
}


/*
 * Debugger
 *
 */
unsigned long Decoder::readCountDEBUG()
{
	return (unsigned long) this->countDEBUG;
}


/*
 * GetFlags
 * 
 * Input: Addr has Address of 7266 counter
 * Returns: Flags of counter
 */
unsigned char Decoder::getFlags()
{
	return inp(XCMD(this->Addr));
}


/*
 * inp
 * 
 * Input: Addr has Address of 7266 counter
 * Returns: data from address
 */
unsigned char Decoder::inp(int Addr)
{
	this->countDEBUG++; // DEBUGGING

	unsigned char Data;						// Declare memory space to store new data
	unsigned char ctrl_chk = 0x01;

	DIRECTION_REG = 0x00; 					// Set PortD DDRD as input

	// Address Data
	ctrl_chk = (Addr & ctrl_chk);			// Isolate C/D Addr Bit

	digitalWrite(this->oPin_CS, LOW);
	digitalWrite(this->oPin_RD, LOW);
	
	// Capture Addressed Data
	if(ctrl_chk)
	{
		digitalWrite(this->oPin_CD, HIGH);	// Select Controls

	} else
	{
		digitalWrite(this->oPin_CD, LOW);	// Select Data
	}
	
	// <-- Add time to give decoder chance to write to bus ************
	/*
	asm("nop");
	*/
	
	delayMicroseconds(DELAY_MICRO);
	Data = INPUT_REG;						// Access contents of Port K address
	delayMicroseconds(DELAY_MICRO);

	digitalWrite(this->oPin_RD, HIGH);
	delayMicroseconds(DELAY_MICRO);
	digitalWrite(this->oPin_CS, HIGH);
	delayMicroseconds(DELAY_MICRO);
	
	return Data;
}


/*
 * outp
 * 
 * Input: 
 *	Addr has Address of 7266 counter
 *  Data has 8 bits
 */
void Decoder::outp(int Addr, unsigned char Data)
{
	this->countDEBUG++; // DEBUGGING

	unsigned char ctrl_chk = 0x01;

	DIRECTION_REG = 0xFF; 					// Set PortD DDRD as output

	// Address Data
	ctrl_chk = (Addr & ctrl_chk);			// Isolate C/D Addr Bit

	// Set Data
	OUTPUT_REG = Data;   					// Output Data to contents of PortK address
	digitalWrite(this->oPin_CS, LOW);
	digitalWrite(this->oPin_WR, LOW);

	// Write to Address Space on Decoder
	if(ctrl_chk)
	{
		digitalWrite(this->oPin_CD, HIGH);	// Select Controls

	} else
	{
		digitalWrite(this->oPin_CD, LOW);	// Select Data
	}
	
	// <-- Add time to give decoder chance to read ************
	/*
	asm("nop");
	*/
	
	delayMicroseconds(DELAY_MICRO);
	digitalWrite(this->oPin_WR, HIGH); // Capture on back-edge of write signal
	delayMicroseconds(DELAY_MICRO);

	digitalWrite(this->oPin_CS, HIGH);
	delayMicroseconds(DELAY_MICRO);
}


unsigned long Decoder::lrotr(unsigned long Data, unsigned char bits)
{
	return ((Data >> bits) | (Data << (32 - bits)));  
}





